
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_param.c
  * @author     baiyang
  * @date       2021-10-19
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu.h"
#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_imu_assign_param()
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    param_link_variable(PARAM_ID(INS, INS_GYROFFS_X), &_imu->_gyro_offset[0].x);
    param_link_variable(PARAM_ID(INS, INS_GYROFFS_Y), &_imu->_gyro_offset[0].y);
    param_link_variable(PARAM_ID(INS, INS_GYROFFS_Z), &_imu->_gyro_offset[0].z);

    param_link_variable(PARAM_ID(INS, INS_GYR2OFFS_X), &_imu->_gyro_offset[1].x);
    param_link_variable(PARAM_ID(INS, INS_GYR2OFFS_Y), &_imu->_gyro_offset[1].y);
    param_link_variable(PARAM_ID(INS, INS_GYR2OFFS_Z), &_imu->_gyro_offset[1].z);

    param_link_variable(PARAM_ID(INS, INS_GYR3OFFS_X), &_imu->_gyro_offset[2].x);
    param_link_variable(PARAM_ID(INS, INS_GYR3OFFS_Y), &_imu->_gyro_offset[2].y);
    param_link_variable(PARAM_ID(INS, INS_GYR3OFFS_Z), &_imu->_gyro_offset[2].z);

    param_link_variable(PARAM_ID(INS, INS_ACCSCAL_X), &_imu->_accel_scale[0].x);
    param_link_variable(PARAM_ID(INS, INS_ACCSCAL_Y), &_imu->_accel_scale[0].y);
    param_link_variable(PARAM_ID(INS, INS_ACCSCAL_Z), &_imu->_accel_scale[0].z);
    param_link_variable(PARAM_ID(INS, INS_ACCOFFS_X), &_imu->_accel_offset[0].x);
    param_link_variable(PARAM_ID(INS, INS_ACCOFFS_Y), &_imu->_accel_offset[0].y);
    param_link_variable(PARAM_ID(INS, INS_ACCOFFS_Z), &_imu->_accel_offset[0].z);

    param_link_variable(PARAM_ID(INS, INS_ACC2SCAL_X), &_imu->_accel_scale[1].x);
    param_link_variable(PARAM_ID(INS, INS_ACC2SCAL_Y), &_imu->_accel_scale[1].y);
    param_link_variable(PARAM_ID(INS, INS_ACC2SCAL_Z), &_imu->_accel_scale[1].z);
    param_link_variable(PARAM_ID(INS, INS_ACC2OFFS_X), &_imu->_accel_offset[1].x);
    param_link_variable(PARAM_ID(INS, INS_ACC2OFFS_Y), &_imu->_accel_offset[1].y);
    param_link_variable(PARAM_ID(INS, INS_ACC2OFFS_Z), &_imu->_accel_offset[1].z);

    param_link_variable(PARAM_ID(INS, INS_ACC3SCAL_X), &_imu->_accel_scale[2].x);
    param_link_variable(PARAM_ID(INS, INS_ACC3SCAL_Y), &_imu->_accel_scale[2].y);
    param_link_variable(PARAM_ID(INS, INS_ACC3SCAL_Z), &_imu->_accel_scale[2].z);
    param_link_variable(PARAM_ID(INS, INS_ACC3OFFS_X), &_imu->_accel_offset[2].x);
    param_link_variable(PARAM_ID(INS, INS_ACC3OFFS_Y), &_imu->_accel_offset[2].y);
    param_link_variable(PARAM_ID(INS, INS_ACC3OFFS_Z), &_imu->_accel_offset[2].z);

    param_link_variable(PARAM_ID(INS, INS_GYRO_FILTER), &_imu->_gyro_filter_cutoff);
    param_link_variable(PARAM_ID(INS, INS_ACCEL_FILTER), &_imu->_accel_filter_cutoff);

    param_link_variable(PARAM_ID(INS, INS_USE), &_imu->_use[0]);
    param_link_variable(PARAM_ID(INS, INS_USE2), &_imu->_use[1]);
    param_link_variable(PARAM_ID(INS, INS_USE3), &_imu->_use[2]);

    param_link_variable(PARAM_ID(INS, INS_STILL_THRESH), &_imu->_still_threshold);
    param_link_variable(PARAM_ID(INS, INS_GYR_CAL), &_imu->_gyro_cal_timing);

    param_link_variable(PARAM_ID(INS, INS_TRIM_OPTION), &_imu->_trim_option);
    param_link_variable(PARAM_ID(INS, INS_ACC_BODYFIX), &_imu->_acc_body_aligned);

    param_link_variable(PARAM_ID(INS, INS_POS1_X), &_imu->_accel_pos[0].x);
    param_link_variable(PARAM_ID(INS, INS_POS1_Y), &_imu->_accel_pos[0].y);
    param_link_variable(PARAM_ID(INS, INS_POS1_Z), &_imu->_accel_pos[0].z);

    param_link_variable(PARAM_ID(INS, INS_POS2_X), &_imu->_accel_pos[1].x);
    param_link_variable(PARAM_ID(INS, INS_POS2_Y), &_imu->_accel_pos[1].y);
    param_link_variable(PARAM_ID(INS, INS_POS2_Z), &_imu->_accel_pos[1].z);

    param_link_variable(PARAM_ID(INS, INS_POS3_X), &_imu->_accel_pos[2].x);
    param_link_variable(PARAM_ID(INS, INS_POS3_Y), &_imu->_accel_pos[2].y);
    param_link_variable(PARAM_ID(INS, INS_POS3_Z), &_imu->_accel_pos[2].z);

    param_link_variable(PARAM_ID(INS, INS_GYR_ID), &_imu->_gyro_id[0]);
    param_link_variable(PARAM_ID(INS, INS_GYR2_ID), &_imu->_gyro_id[1]);
    param_link_variable(PARAM_ID(INS, INS_GYR3_ID), &_imu->_gyro_id[2]);

    param_link_variable(PARAM_ID(INS, INS_ACC_ID), &_imu->_accel_id[0]);
    param_link_variable(PARAM_ID(INS, INS_ACC2_ID), &_imu->_accel_id[1]);
    param_link_variable(PARAM_ID(INS, INS_ACC3_ID), &_imu->_accel_id[2]);

    param_link_variable(PARAM_ID(INS, INS_FAST_SAMPLE), &_imu->_fast_sampling_mask);
    param_link_variable(PARAM_ID(INS, INS_ENABLE_MASK), &_imu->_enable_mask);
    param_link_variable(PARAM_ID(INS, INS_GYRO_RATE), &_imu->_fast_sampling_rate);
}

void sensor_imu_gyr_offset_id_variable_to_param()
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYROFFS_X), _imu->_gyro_offset[0].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYROFFS_Y), _imu->_gyro_offset[0].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYROFFS_Z), _imu->_gyro_offset[0].z);

    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR2OFFS_X), _imu->_gyro_offset[1].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR2OFFS_Y), _imu->_gyro_offset[1].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR2OFFS_Z), _imu->_gyro_offset[1].z);

    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR3OFFS_X), _imu->_gyro_offset[2].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR3OFFS_Y), _imu->_gyro_offset[2].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR3OFFS_Z), _imu->_gyro_offset[2].z);

    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR_ID), _imu->_gyro_id[0]);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR2_ID), _imu->_gyro_id[1]);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_GYR3_ID), _imu->_gyro_id[2]);
}

void sensor_imu_acc_scale_offset_id_variable_to_param()
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACCSCAL_X), _imu->_accel_scale[0].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACCSCAL_Y), _imu->_accel_scale[0].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACCSCAL_Z), _imu->_accel_scale[0].z);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACCOFFS_X), _imu->_accel_offset[0].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACCOFFS_Y), _imu->_accel_offset[0].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACCOFFS_Z), _imu->_accel_offset[0].z);

    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC2SCAL_X), _imu->_accel_scale[1].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC2SCAL_Y), _imu->_accel_scale[1].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC2SCAL_Z), _imu->_accel_scale[1].z);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC2OFFS_X), _imu->_accel_offset[1].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC2OFFS_Y), _imu->_accel_offset[1].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC2OFFS_Z), _imu->_accel_offset[1].z);

    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC3SCAL_X), _imu->_accel_scale[2].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC3SCAL_Y), _imu->_accel_scale[2].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC3SCAL_Z), _imu->_accel_scale[2].z);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC3OFFS_X), _imu->_accel_offset[2].x);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC3OFFS_Y), _imu->_accel_offset[2].y);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC3OFFS_Z), _imu->_accel_offset[2].z);

    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC_ID), _imu->_accel_id[0]);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC2_ID), _imu->_accel_id[1]);
    param_set_and_save_ifchanged(PARAM_ID(INS, INS_ACC3_ID), _imu->_accel_id[2]);
}

/*------------------------------------test------------------------------------*/


